#include <gtest/gtest.h>
#include <fstream>
#include "axisgroup/ag_config.h"
#include "jkutil/iniparser.h"
#include "config.h"

using axisgroup::ConfigItem;

class AgConfigTest : public ::testing::Test
{
public:
    virtual void SetUp()
    {
        usr = ZUC_USER_CONFIG_FILE(robconfig_test.ini);
        cfg = std::make_shared<axisgroup::AgConfig>();
        create_usr();
    }

    virtual void TearDown() { std::remove(usr.c_str()); }

    void create_usr()
    {
        std::string data = R"(
[ROBOT]
SERIAL_NUM=12100091

[TIO]
DI_PIN_CFG = 0
DO_PIN_CFG = 0
AI_PIN_CFG = 0
RS485_CHN1_MODE = 1
RS485_CHN2_MODE = 1
RS485_CHN1_COMM = {0, 115200, 8, 1, 78}
RS485_CHN2_COMM = {0, 115200, 8, 1, 78}
VOUT_ENABLE = 1
VOUT_VOL_CFG = 0

[TRAJ]
TCP_MAX_VEL={2000.000,180}
TCP_MAX_ACC={3500.000,360}
TCP_MAX_JERK={0,0}

[JOINT]
JOINT_MIN_POS_LIMIT = {-360.0, -85.0, -175.0, -85, -360.0, -360.0}
JOINT_MAX_POS_LIMIT = {360.0, 265.0, 175.0, 265, 360.0, 360.0}
JOINT_MAX_VEL = {120.0, 120.0, 120.0, 180.0, 180.0, 180.0}
JOINT_MAX_ACC = {400.0, 400.0, 400.0, 400.0, 400.0, 400.0}
JOINT_MAX_JERK=0
JOINT_ERR_LIMIT = {80, 80, 80, 80, 80, 80}

[JOG]
JOG_MAX_TRAJ_VEL={250.000,180}
JOG_MAX_TRAJ_ACC={3500.000,360}
JOG_MAX_TRAJ_JERK={0,0}
JOG_MAX_JOINT_VEL={180.0, 180.0, 180.0, 180.0, 180.0, 180.0}
JOG_MAX_JOINT_ACC={720.0, 720.0, 720.0, 720.0, 720.0, 720.0}
JOG_MAX_JOINT_JERK=0
POS_JOG_ACC={50.0, 50.0, 10.0, 10.0, 5.0, 5.0, 5.0}
ORI_JOG_ACC={20.0, 20.0, 10.0, 10.0, 5.0, 5.0, 5.0}

[SERVOMOVE]
FILTER_TYPE=4
MMF_MAX_BUF=20
MMF_KP=0.1
MMF_KV=0.2
MMF_KA=0.6
NLF_MAX_VP=1000
NLF_MAX_AP=4000
NLF_MAX_JP=8000
NLF_MAX_VR=120
NLF_MAX_AR=180
NLF_MAX_JR=720
LPF_CUTOFF_FREQ=5

[STOP]
CAT1_STOP_TRAJ_ACC={3500.000,360}
CAT2_STOP_TRAJ_ACC={3500.000,360}
CAT1_STOP_JOINT_ACC=720
CAT2_STOP_JOINT_ACC=720

[PAYLOAD]
CURR_PAYLOAD_ID=0
SENSOR_PAYLOAD=0

[MANUEL]
TCP_ID=0
USR_FRAME_ID=0

[BASEOFFSET]
TRANS={0.0, 0.0, 0.0}
ORI={0.0, 0.0, 0.0}

[DHPARAM]
DH_PARAM={196.5, -158.0, 144.15, 126.65, 819, 687}

[SERVOPARAM]
TORQUE_FORWARD_ENABLE=1

[DEFAULTPOSE]
SHRUNKENPOSE={-90.0, 0.0, 152.0, 120.0, 0.0, 0.0, 0}
STRETCHEDPOSE={0.0, 90.0, 0.0, 90.0, 180.0, 0.0, 0}
INITIALPOSE={-90.0, 0.0, 152.0, 120.0, 0.0, 0.0, 0}
POSETOL=0.1
HOME_VEL=30.0

[TORQSENSOR]
ENABLE=0
COMM_TYPE=0
SENSOR_IP=192.168.2.228
SENSOR_PORT=49152
SENSOR_BRAND=1
SENSOR_BAUDRATE=9600
SENSOR_DATABIT=8
SENSOR_STOPBIT=1
SENSOR_PARITY=0
[TORQUESENSORFILTER]
HZ=0.0
[ADMITCTRLCONFIG]
ADMITCTRLCONFIG_1={1, 0.0, 5.0, 0.0, 0.0, 0.0}
ADMITCTRLCONFIG_2={2, 0.0, 5.0, 0.0, 0.0, 0.0}
ADMITCTRLCONFIG_3={3, 0.0, 5.0, 0.0, 0.0, 0.0}
ADMITCTRLCONFIG_4={4, 0.0, 0.2, 0.0, 0.0, 0.0}
ADMITCTRLCONFIG_5={5, 0.0, 0.2, 0.0, 0.0, 0.0}
ADMITCTRLCONFIG_6={6, 0.0, 0.2, 0.0, 0.0, 0.0}
ADMITCTRLFRAME=0
[VELCOMPLIANTCTRLLEVEL]
VELCOMPLIANTCTRL={0.0, 0.0, 0.0, 0.0, 0.0}
[COMPLIANTCONDITION]
FT={0.0, 0.0, 0.0, 0.0, 0.0}
[COMPLIANTTYPEENABLE]
COMPLIANT_TYPE=0
COMPLIANT_ENABLE=1
[TORQUESENSORSOFTLIMIT]
Fx=0
Fy=0
Fz=0
Mx=0
My=0
Mz=0
[ADMITTANCEDRAGDEADZONE]
FX=1
FY=1
FZ=1
MX=0.03
MY=0.03
MZ=0.03

[DRAGCONTROL]
DRAG_CONTROL_FILTER_FACTOR={30, 30, 30, 30, 30, 30}
DRAG_CONTROL_FV_COMPENSATE={80, 80, 80, 80, 80, 80}
DRAG_CONTROL_FC_COMPENSATE={80, 80, 80, 80, 80, 80}
DRAG_CONTROL_ENABLE=0
DRAG_CONTROL_DITHER_FACTOR={1.5, 1.5, 1.5, 1.5, 1.5, 1.5}

[PAYLOADIDENTIFICATION]
TRAJ_START_0={0, 90, 0, -40, 180, -40, 0}
TRAJ_START_1={0, 90, 0, -40, 180, 50, 0}
TRAJ_START_2={0, 90, 0, 0, 172, 0, 0}
TRAJ_START_3={0, 90, 0, 0, 180, 0, 0}
TRAJ_END_0={0, 90, 0, 40, 180, 40, 0}
TRAJ_END_1={0, 90, 0, 40, 180, 130, 0}
TRAJ_END_2={0, 90, 0, 0, 188, 0, 0}
TRAJ_END_3={0, 90, 0, 0, 180, 0, 0}

[FRICTION]
FRICTION_IDENTIFY_JOINT_INDEX=0
FRICTION_IDENTIFY_END_POS_1={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_END_POS_2={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_END_POS_3={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_END_POS_4={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_END_POS_5={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_END_POS_6={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_END_POS_7={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_START_POS_1={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_START_POS_2={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_START_POS_3={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_START_POS_4={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_START_POS_5={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_START_POS_6={1, 2, 3, 4, 5, 6}
FRICTION_IDENTIFY_START_POS_7={1, 2, 3, 4, 5, 6}

[MOTSAFE_COLLISION]
REACTION_TYPE=0
REBOUND_ANGLE=3
REBOUND_LENGTH=30
REBOUND_J_ACC=2880
REBOUND_J_VEL=720
REBOUND_L_ACC=4000
REBOUND_L_VEL=2000
COLLISION_METHOD=1
GM_FILTER=20
GM_THRESHOLD={1.0, 1.05, 1.1, 1.3, 1.5}
JOINT_SENSITIVITY={50, 50, 50, 50, 50, 50, 50}
FORCE_LIMIT_VALUE=50
COLLISION_DETECTION_ENABLE=1
REBOUND_TRAJ_ACC = {8000,1080}
REBOUND_JOINT_ACC = 1080


[MOTSAFE]
CLSN_SENSITIVITY=5
MOMENTUM_LIMIT=70.000000
TCP_VEL_LIMIT=2000.000000
ELBOW_VEL_LIMIT=2000.000000
ROBOT_POWER_LIMIT=2000.000000
STOPPING_DIST=4000.000000
STOPPING_TIME=5.000000

[MOTSAFE_REDUCE]
CLSN_SENSITIVITY=5
TCP_SPEED_LIMIT_REDUCE=250
ELBOW_VEL_LIMIT_REDUCE=250
MOMENTUM_LIMIT_REDUCE=0.000000
ROBOT_POWER_LIMIT_REDUCE=0.000000
STOPPING_DIST_REDUCE=4000
STOPPING_TIME_REDUCE=5

[MOTSAFE_ATTTITUDE]
LIMIT_ATTITUDE_OPEN=0
LIMIT_ATTITUDE_AUTO=0
CONICAL_TIP_CHOICE=0
TOOL_PROPERTIES_TILT=0
TOOL_PROPERTIES_PAN=0
DEVIATION=30
DEVIATION_WARNING=20
CONICAL_CENRPY={180.000000,0.000000,90.000000}

[SERVOSTATISTIC]
STATISTIC_DATE=0
WARN_8384_ALL_CNT={0,0,0,0,0,0}
WARN_8384_DALIY_CNT={0,0,0,0,0,0}
WARN_8385_ALL_CNT={0,0,0,0,0,0}
WARN_8385_DALIY_CNT={0,0,0,0,0,0}
WARN_8387_ALL_CNT={0,0,0,0,0,0}
WARN_8387_DALIY_CNT={0,0,0,0,0,0}
WARN_8388_ALL_CNT={0,0,0,0,0,0}
WARN_8388_DALIY_CNT={0,0,0,0,0,0}
WARN_7001_ALL_CNT={0,0,0,0,0,0}
WARN_7001_DALIY_CNT={0,0,0,0,0,0}
WARN_THRSEHOLD={50,50,50,50,50,50}
)";
        std::ofstream ofs(usr);
        ofs << data;
        ofs.close();
    }
    void create_sys() {}

    std::shared_ptr<axisgroup::AgConfig> cfg;
    std::string usr, sys;
};
TEST_F(AgConfigTest, load_usr)
{
    //
    ASSERT_EQ(7, cfg->user_items_.size());
    ASSERT_EQ(cfg->load_usr_config("robconfig_test.ini"), 0);

    EXPECT_DOUBLE_EQ(cfg->get_robot_config().traj.vel.trans, 2000);
    EXPECT_DOUBLE_EQ(cfg->get_robot_config().traj.vel.ori, 180);

    EXPECT_DOUBLE_EQ(cfg->get_robot_config().joint_limit.max[0], 360);
    EXPECT_DOUBLE_EQ(cfg->get_robot_config().joint_limit.max[2], 175.0);
    EXPECT_DOUBLE_EQ(cfg->get_robot_config().joint_limit.max[5], 360.0);
    EXPECT_DOUBLE_EQ(cfg->get_robot_config().joint_limit.min[0], -360);
    EXPECT_DOUBLE_EQ(cfg->get_robot_config().joint_limit.min[2], -175.0);
    EXPECT_DOUBLE_EQ(cfg->get_robot_config().joint_limit.min[5], -360);

    //TODO: add more tests
}

TEST_F(AgConfigTest, load_sys)
{
    //TODO: add more tests
    ASSERT_EQ(cfg->load_model_config(sys), 0);
}

TEST_F(AgConfigTest, write_usr)
{
    ASSERT_EQ(cfg->load_usr_config("robconfig_test.ini"), 0);
    cfg->get_robot_config().traj.vel.trans = 1234;
    ASSERT_EQ(cfg->write_usr_config_to_file(), 0);

    axisgroup::AgConfig r;
    ASSERT_EQ(r.load_usr_config("robconfig_test.ini"), 0);
    EXPECT_DOUBLE_EQ(r.get_robot_config().traj.vel.trans, 1234);

    //TODO: add more tests
}